import numpy as np
from scipy.spatial.transform import Rotation as R


def get_relative_rotation(q1, q2):

    # [w, x, y, z]
    r1 = R.from_quat([q1[3], q1[0], q1[1], q1[2]])
    r2 = R.from_quat([q2[3], q2[0], q2[1], q2[2]])

    # q1^-1 * q2
    relative_rot = r1.inv() * r2

    return relative_rot.as_matrix()


if __name__ == "__main__":

    # Sample Usage
    # [x, y, z, w]
    sensor1_quat = [0.7071, -0.7071, 0, 0]
    sensor2_quat = [0, 0, 0, 1]

    rot_matrix = get_relative_rotation(sensor1_quat, sensor2_quat)
    print("Relative R\n", rot_matrix)
